/*
 * This file is part of the Try! Engine project.
 *
 * Copyright 2010-2011 Emanuele Bertoldi. All rights reserved.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * You should have received a copy of the modified BSD License along with this
 * program. If not, see <http://www.opensource.org/licenses/bsd-license.php>
 */

#include "steering.h"

using namespace Try;

Steering::Steering(BehaviorTree* parent, float max_speed, const Try::Vector3& vel)
    :   Action(parent),
        m_vel(vel),
        m_maxSpeed(max_speed)
{
}

Steering::Steering(float max_speed, const Try::Vector3& vel, BehaviorTree* parent)
    :   Action(parent),
        m_vel(vel),
        m_maxSpeed(max_speed)
{
}

Steering::~Steering()
{
}

Try::Vector3 Steering::velocity() const
{
    return m_vel;
}

float Steering::maxSpeed() const
{
    return m_maxSpeed;
}

float Steering::speed() const
{
    return m_vel.length();
}

void Steering::setMaxSpeed(float max_speed)
{
    m_maxSpeed = max_speed;
}

Try::Vector3 Steering::seek(const Try::Vector3& pos, const Try::Vector3& target)
{
    Try::Vector3 desired_vel = Try::Vector3(target - pos).normalised() * m_maxSpeed;

    return (desired_vel - m_vel);
}

Try::Vector3 Steering::flee(const Try::Vector3& pos, const Try::Vector3& target)
{
    Try::Vector3 desired_vel = Try::Vector3(pos - target).normalised() * m_maxSpeed;

    return (desired_vel - m_vel);
}

Try::Vector3 Steering::arrive(const Try::Vector3& pos, const Try::Vector3& target, float slowing_distance)
{
    Try::Vector3 target_offset = target - pos;

    float distance = target_offset.length();
    if (distance > 0)
    {
        float speed = m_maxSpeed * (distance / slowing_distance);
        speed = std::min(speed, m_maxSpeed);
        Try::Vector3 desired_vel = target_offset * (speed / distance);

        return (desired_vel - m_vel);
    }

    return Try::Vector3::Zero;
}
